// MultiStepper
// -*- mode: C++ -*-
//
// Control both Stepper motors at the same time with different speeds
// and accelerations. 
// Requires the AFMotor library (https://github.com/adafruit/Adafruit-Motor-Shield-library)
// And AccelStepper with AFMotor support (https://github.com/adafruit/AccelStepper)
// Public domain!

#include <AccelStepper.h>
#include <AFMotor.h>

// two stepper motors one on each port
AF_Stepper motor1(200, 1);
AF_Stepper motor2(200, 2);

// you can change these to SINGLE, DOUBLE or INTERLEAVE or MICROSTEP!
// wrappers for the first motor!
void forwardstep1() {  
  motor1.onestep(FORWARD,  MICROSTEP);
}
void backwardstep1() {  
  motor1.onestep(BACKWARD,  MICROSTEP);
}
// wrappers for the second motor!
void forwardstep2() {  
  motor2.onestep(FORWARD,  MICROSTEP);
}
void backwardstep2() {  
  motor2.onestep(BACKWARD,  MICROSTEP);
}

// Motor shield has two motor ports, now we'll wrap them in an AccelStepper object
AccelStepper stepper1(forwardstep1, backwardstep1);
AccelStepper stepper2(forwardstep2, backwardstep2);

long motorPosition[2];
unsigned long time = 0;
unsigned long loopCounter = 0;

void setup()
{  
    stepper1.setMaxSpeed(600.0);
    stepper1.setAcceleration(400.0);
    
    stepper2.setMaxSpeed(600.0);
    stepper2.setAcceleration(400.0);

    pinMode(14, INPUT);
    pinMode(15, INPUT);
    
    //stepper2.moveTo(1000000);
    motorPosition[0] = 0;
    motorPosition[1] = 0;
    Serial.begin(9600);
    time = millis();
    //establishContact();  // send a byte to establish contact until receiver responds     
}

int i = 0;
int inChar;
String inString = "";

void sendState()
{
   
   char s[255] = "s:mp1:";
   long motor1CurrentPosition = stepper1.currentPosition();
   long motor2CurrentPosition = stepper2.currentPosition();
   char s1[11] = "";
   ltoa(motor1CurrentPosition, s1, 10);
   strcat( s, s1 );
   strcat ( s,";mp2:" );
   ltoa(motor2CurrentPosition, s1, 10);
   strcat( s, s1);
   strcat (s,";ss1:");
   itoa(digitalRead(14), s1, 10);
   strcat( s, s1);   
   strcat (s,";ss2:");
   itoa(digitalRead(15), s1, 10);
   strcat( s, s1);      
   strcat (s,";");
   Serial.flush();
   Serial.println( s ); 
   //delay(1);
}

void parseCommand()
{
  char s[11] = "";
  // Разбор команды установки угла ротора двигателя в шагах
  if (isAlpha(inString.charAt(0)) && inString.charAt(0) == 'A')
  {
    // Номера двигателей 0 и 1
    if (inString.charAt(1) == '0' || inString.charAt(1) == '1')
    {
      // Определение номера двигателя
      byte motorNumber = (byte)inString.charAt(1) - '0';
      // Выделение числа шагов относительно начала
      inString.substring(2).toCharArray(s, sizeof(s)); 
      motorPosition[motorNumber] = atol( s );
      //Serial.println(s);
    }
  }
  // Сброс счетчика шагов в 0
  else if (isAlpha(inString.charAt(0)) && inString.charAt(0) == 'R')
  {
    byte motorNumber = (byte)inString.charAt(1) - '0';
    if (motorNumber == 0 )
    {
        //Serial.println("Resetted");
        motorPosition[motorNumber] = 0;
        stepper1.setCurrentPosition(0);
        stepper1.setMaxSpeed(800.0);
        stepper1.setAcceleration(600.0);
      }
      else if (motorNumber == 1 )
      
      {
        motorPosition[motorNumber] = 0;
        stepper2.setCurrentPosition(0);
        stepper2.setMaxSpeed(800.0);
        stepper2.setAcceleration(600.0);
      }
    }
}

boolean stringComplete = false;

void loop()
{
    stepper1.moveTo(motorPosition[0]);
    stepper2.moveTo(motorPosition[1]);
    stepper1.run();
    stepper2.run();
  // Read serial input:
  while (Serial.available() > 0 && stringComplete == false) 
  {
      inChar = Serial.read();
      inString += (char)inChar;
      if (inChar == '\n')
      {
        //Serial.print(inString);
        stringComplete = true;
      }
  }
  if (stringComplete)
  {
        parseCommand();
        inString = "";
        stringComplete = false;
        //break;
  }
  if (millis() - time > 100)
  {
    sendState();
    time = millis();
  }
  loopCounter++;
}


